#include "JY61P.h"
#include "IMU.h"
#include "hardwareDef.h"

uint8_t s_JY[12] = {};
uint8_t i_JY = 0;

int16_t w0, w1;
int16_t theta0, theta1;
bool jy61new = 0;

void INIT_JY61(void)
{//写格式：0xFF 0xAA 寄存器地址 数据低 数据高
  JY61_UNLOCK();

  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x02);//输出内容
  IMU_TX(SC16IS752_CHANNEL_B, 0b00001100);//角度和角速率
  IMU_TX(SC16IS752_CHANNEL_B, 0b00000000);

  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x02);//输出速率
  IMU_TX(SC16IS752_CHANNEL_B, 0b1000);//50Hz
  IMU_TX(SC16IS752_CHANNEL_B, 0x00);

  JY61_OFFLIGHT();

  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x1F);//带宽
  IMU_TX(SC16IS752_CHANNEL_B, 0b0011);//42Hz
  IMU_TX(SC16IS752_CHANNEL_B, 0x00);

  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x24);//算法
  IMU_TX(SC16IS752_CHANNEL_B, 0b1);//6轴算法
  IMU_TX(SC16IS752_CHANNEL_B, 0x00);

  JY61_ONLIGHT();

  JY61_SAVE();
}

void JY61_RX(uint8_t in)
{
  static uint16_t checksum;
  static uint8_t type;
  switch (i_JY)
  {
  case 0://检测帧头
    if (in == 0x55)
    {
      for (uint8_t j = 0; j < 12; j ++)
      {
        s_JY[j] = 0;
      }
      s_JY[i_JY] = in;
      i_JY ++;
    }
    break;

  case 1://数据内容
    switch (in)
    {
    case 0x52:
      type = in;
      s_JY[i_JY] = in;
      i_JY ++;
      break;

    case 0x53:
      type = in;
      s_JY[i_JY] = in;
      i_JY ++;
      break;

    default:
      type = 0;
      i_JY = 0;
      break;
    }
    break;

  default:
    s_JY[i_JY] = in;
    i_JY ++;
    break;

  case 10:
    checksum = 0;
    for (uint8_t j = 0; j < 10; j ++)
    {
      checksum += s_JY[j];
    }
    if (in == (checksum & 0xFF))
    {
      s_JY[i_JY] = in;
      if (type == 0x52)
      {
        w0 = s_JY[2];
        w0 += (s_JY[3] << 8);
        w1 = s_JY[4];
        w1 += (s_JY[5] << 8);
        jy61new = 1;
      }
      else if (type == 0x53)
      {
        theta0 = s_JY[2];
        theta0 += (s_JY[3] << 8);
        theta1 = s_JY[4];
        theta1 += (s_JY[5] << 8);
      }
      type = 0;
      i_JY = 0;
    }
    break;
  }
}

int32_t JY61_W0(void)
{//第0轴数据返回，这里是俯仰数据
  return (int32_t)w0 * 1;// 250/250，俯仰速率250
}

int32_t JY61_W1(void)
{//第1轴数据返回，这里是横滚数据
  return (int32_t)w1 * 5 / 6;// 250/300，横滚速率300
}

int32_t JY61_THETA0(void)
{
  return (int32_t)theta0 * 3 / 4;
}

int32_t JY61_THETA1(void)
{
  return (int32_t)theta1 / 2;
}

bool JY61_UPDATED(void)
{
  bool old = jy61new;
  jy61new = 0;
  return old;
}

void JY61_UNLOCK(void)
{
  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);//解锁设置
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x69);
  IMU_TX(SC16IS752_CHANNEL_B, 0x88);
  IMU_TX(SC16IS752_CHANNEL_B, 0xB5);
}

void JY61_SAVE(void)
{
  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);//保存设置
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x00);
  IMU_TX(SC16IS752_CHANNEL_B, 0x00);
  IMU_TX(SC16IS752_CHANNEL_B, 0x00);
}

void JY61_OFFLIGHT(void)
{
  //写格式：0xFF 0xAA 寄存器地址 数据低 数据高
  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x1B);//LED开关
  IMU_TX(SC16IS752_CHANNEL_B, 0b1);//关闭
  IMU_TX(SC16IS752_CHANNEL_B, 0x00);
}

void JY61_ONLIGHT(void)
{
  //写格式：0xFF 0xAA 寄存器地址 数据低 数据高
  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x1B);//LED开关
  IMU_TX(SC16IS752_CHANNEL_B, 0b0);//打开
  IMU_TX(SC16IS752_CHANNEL_B, 0x00);
}

void JY61_CALB(void)
{//写格式：0xFF 0xAA 寄存器地址 数据低 数据高
  JY61_UNLOCK();

  uint16_t zeroRef = JY61_THETA0();
  uint16_t oneRef = JY61_THETA1();
  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x79);//0轴角度参考
  IMU_TX(SC16IS752_CHANNEL_B, zeroRef & 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, (zeroRef >> 8));

  IMU_TX(SC16IS752_CHANNEL_B, 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, 0xAA);
  IMU_TX(SC16IS752_CHANNEL_B, 0x7A);//1轴角度参考
  IMU_TX(SC16IS752_CHANNEL_B, oneRef & 0xFF);
  IMU_TX(SC16IS752_CHANNEL_B, (oneRef >> 8));

  JY61_SAVE();
}
